//
// AStar
//

import java.util.ArrayList;
import java.util.Collections;
import java.util.HashMap;
import java.util.TreeMap;

class SearchRTAStar implements Search
{
	static boolean debug = false;	
	protected Heuristic heuristic = new RushHourHeuristic3();//hardcoded now, but should be dynamic
	protected HashMap<Long, Integer> costMap = new HashMap<Long, Integer>();
    protected long m_nodesExpanded;

    // Get number of nodes expanded.
	public long getNodesExpanded()
    {
        return m_nodesExpanded;
    }

	public int costFunction(int cost, State s) {
		long stateId = s.getStateID();
		if(costMap.containsKey(stateId)) {
			return cost + costMap.get(stateId);
		}
		else {			
			int totalCost = cost + heuristic.getHeuristic(s);
			costMap.put(stateId, totalCost);
			return totalCost;
		}		
	}

	
	// Input: start state
	// Output: cost of solution and the solution path.
	// Returns: true if solution found, otherwise false.
    public boolean run( State s, Cost cost, ArrayList<Action> solution )
    {
        long startStateId = s.getStateID();
    	heuristic.init(s);    	
    	ArrayList<Action> childActions = new ArrayList<Action>();
    	ArrayList<Long> stateSequence = new ArrayList<Long>();
    	stateSequence.add(startStateId);
        m_nodesExpanded = 0;
        boolean solved = s.isGoal();//just in case the first state is the goal state
        while(!solved) {        	        	        	 
        	if(Runtime.getRuntime().freeMemory()<15000) {
        		solution.clear();
        		System.out.println("Running low on memory, giving up");
        		System.out.println("Nodes expanded: " + m_nodesExpanded);
        		System.out.println("Costmap size: " + costMap.size());
        		break;
        	}
        	
        	//test the cost for each child action
        	int min = Integer.MAX_VALUE;
        	int nextMin = Integer.MAX_VALUE;
        	Action bestAction = null;
        	childActions.clear();
       		s.getActions(childActions);
       		m_nodesExpanded++;
       		for(Action action: childActions) {
       			int actionCost = s.getCost(action);       				
       			s.make(action);
            	if(s.isGoal()) {
            		solution.add(action);
            		solved = true;
            		break;
            	}
       			       			
       			int estimatedTotalCost = costFunction(actionCost, s);
       			if(estimatedTotalCost<min) {
       				min = estimatedTotalCost;
       				bestAction = action;
       			}
       			else if(estimatedTotalCost<nextMin) {
       				nextMin = estimatedTotalCost;
       			}       			
       			s.retract(action);       			
       		}        	
       		
       		if(!solved) {
	       		//update cost of current state and add the selected action to the path
	       		long stateId = s.getStateID();
	       		costMap.put(stateId, nextMin);
	       		s.make(bestAction);
	       		//solution.add(bestAction);
	       		stateSequence.add(stateId);
	       		if(debug) {
	       			s.draw();
	       		}
       		}
        }

        int totalCost = 0;
        if(solved) {        	
        	for(int i=0; i<stateSequence.size()-1; i++) {        		
		        s.setState(stateSequence.get(i));
		        childActions.clear();
		        s.getActions(childActions);
		        for(Action action: childActions) {
		        	s.make(action);
		        	if(s.getStateID()==stateSequence.get(i+1)) {
		        		solution.add(action);
		        		break;
		        	}
		        	s.retract(action);
		        }		        
        	}
	        for(Action action: solution) {
	        	totalCost += s.getCost(action);
	        	s.make(action);
	        }
        }
        System.out.println("Heuristic call count: " + heuristic.getCallCount());
        cost.value = totalCost;
        return solved;
    }    
}
